/* Copyright(C): Leaf Johnson 2009,2010

    This file is part of leaf-lib.

    leaf-lib is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    leaf-lib is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with leaf-lib.  If not, see <http://www.gnu.org/licenses/>.


 * 
 * 
 */


#include <devices_ext/promotion.h>
#include <devices/uart.h>
#include <string.h>
#include <stdlib.h>
#include <util/delay.h>

/*广播，发给所有port上的驱动器*/
void prom_broadcast(PromPort p, const char *cmd){
	uart_write( p, cmd, strlen(cmd) );
}

/*在调用之前一定要初始化好串口*/
void prom_init(Promotion *p, PromPort port, PromId id){
	p->port = port;
	p->id = id;
}

static const size_t MAX_ADRLEN = 3;
/*向promotion发送指令*/
void prom_send_command(Promotion *p, const char *cmd){
	size_t cmdlen = strlen(cmd);
	/*为网络连接方式时，添加地址*/
	if(p->id != PROM_SINGLE){
		char buf[ cmdlen + MAX_ADRLEN + 1 ];
		itoa( p->id, buf, 10 );
		strcat( buf, cmd );
		cmdlen = strlen(buf);
		buf[ cmdlen ] = 0x0d;
		uart_write( p->port, buf, cmdlen + 1 );
	}
	else{
		char buf[ cmdlen + 1 ];
		memcpy( buf, cmd, cmdlen );
		buf[ cmdlen ] = 0x0d;
		uart_write( p->port, buf, cmdlen + 1 );
	}
}

/* 便捷的指令生成法，外部不能调用 */
static inline void prom_int_command(char *buf, const char prefix, int param){
	buf[0] = prefix;
	itoa(param, &buf[1], 10);
}
/* 当指令头不为单字符时，用这个函数生成指令，外部也不能调用 */
static inline void prom_long_command(char *buf, const char *prefix, long param){
	strcpy(buf, prefix);
	ltoa(param, &buf[strlen(prefix)], 10);
}

/*发送速度指令*/
void prom_speed(Promotion *p, speed_t setspeed){
	char buf[9];
	prom_int_command(buf, 'V', setspeed);
	prom_send_command(p, buf);
}

/*发送电压模式的指令*/
void prom_votage(Promotion *p, int16_t v){
	char buf[9];
	prom_int_command(buf, 'U', v);
	prom_send_command(p, buf);
}

/*发送电流模式的指令*/
void prom_current(Promotion *p, int16_t c){
	char buf[9];
	prom_int_command(buf, 'C', c);
	prom_send_command(p, buf);
}

/*发送位移指令*/
void prom_move(Promotion *p, dist_t dist){
	char buf[11] = "LR";
	ltoa(dist, &buf[2], 10);
	prom_long_command(buf, "LR", dist);
	prom_send_command(p, buf);
	_delay_us( 700 );
	prom_send_command(p, "M");
}

/*发送绝对位移指令*/
void prom_position(Promotion *p, dist_t dist){
	char buf[11] = "LA";
	ltoa(dist, &buf[2], 10);
	prom_send_command(p, buf);
	_delay_us( 700 );
	prom_send_command(p, "M");
}

/*先改变port上所有驱动器的波特率，再改变此串口的波特率*/
void prom_change_baud(PromPort p, uint32_t baudrate){
	char buf[12] = "BAUD";
	ltoa(baudrate, &buf[4], 10);
	prom_broadcast(p, buf);
	uart_set_baud(p, baudrate);
}

/*先改变驱动器的ID，再改变单片机方面的设置*/
void prom_change_id(Promotion *p, PromId id){
	char buf[11] = "NODEADR";
	itoa(id, &buf[7], 10);
	prom_send_command(p, buf);
	p->id = id;
}


